#!/usr/bin/env python
import roslib; roslib.load_manifest('ebotMain')
import rospy
import serial
import math
from std_msgs.msg import String 

class velocity_calculator:
# Takes tachometer info from arduino board at /dev/robot/arduino 
# and calculates linear and angular robot velocities from it by first converting them into
# left and right wheel velocities
	

	def __init__(self):	# Initialization method
		self.a = serial.Serial('/dev/robot/arduino', 9600)
		
		self.WHEEL_RADIUS = 3*(2.54)/100 # 3 inches in meters
		self.AXIS_DIST    = 23.5*(2.54)/100 # 23.5 inches in meters
		self.TIME_INCREMENT = 50000 # time increment in microseconds (50000 is half a second)
		# this is the time between serial info being sent from arduino about encoder data
		rospy.init_node('velocity_calculator')
		self.velPub = rospy.Publisher('robot_vels', String) 
		self.wPub = rospy.Publisher('wheel_vels', String)


	def read_arduino(self): 	
		
		while not rospy.is_shutdown(): # need this line?
			
			blipPS = self.a.readline();	### Get 'xxx 1/0 yyy 1/0 zzzzzz\r\n'
			blipPS = blipPS.strip('\r\n')   ### Get 'xxx 1/0 yyy 1/0 zzzzzz'
			blipPS = blipPS.split(' ') 	### Get 'xxx', '1/0', 'yyy', '1/0', 'zzzzzz'

			# The exception catchers below catch when 
			# blank info comes through the serial port and 
			# force the info to be 0 meters moved in 500000 microseconds
			# blipsPS has the form [m1blips][0/1][m2blips][0/1][dtime]

			# capture motor 1's encoder data
			try: 
				blips_m1 = int(blipPS[0]) # blips read from tach 1
			except ValueError: # happens when serial port gives nothing
				blips_m1 = 0; # when in doubt, assume not moving
				print "non-printable text read: forcing blips_m1 to 0"	
			except IndexError: # happens when ser port gave nothing, so splitting blipsPS gives an array with none or one val
				blips_m1 = 0; # assume not moving
				print "Index error: forcing blips_m1 to 0"	

			# capture motor 1's direction
			try: 
				dir_m1 = int(blipPS[1])  # direction of m1 -> 0 means forward, 1 means backward
			except ValueError:
				dir_m1 = 0; #assume forward
				print "non-printable text read: assume m1's direction is forward"
			except IndexError: 
				dir_m1 = 0; # assume not moving
				print "Index error: assume m1's direction is forward"	
	
			# capture motor 2's encoder data
			try: 
				blips_m2 = int(blipPS[2]) # blips read from tach 2
			except ValueError:
				blips_m2 = 0; # when in doubt, assume not moving
				print "non-printable text read: forcing blips_m2 to 0"

			except IndexError:
				blips_m2 = 0; # assume not moving
				print "Index error: forcing blips_m2 to 0"	

			# capture motor 2's direction
			try: 
				dir_m2 = int(blipPS[3])
			except ValueError:
				dir_m2 = 0; # assume forward
				print "non-printable text read: forcing blips_m1 to 0"	
			except IndexError: 
				dir_m2 = 0; # assume not moving
				print "Index error: assume m2's direction is forward"	

			# capture time increment from 
			try: 
				d_time = int(blipPS[4])
			except ValueError:
				d_time = self.TIME_INCREMENT
				print "non-printable text read: forcing d_time to default"

			except IndexError:
				d_time = self.TIME_INCREMENT
				print "Index error, d_time is now default"	

			# send all these values to the vel calculation method
			self.blips_to_mps(blips_m1, dir_m1, blips_m2, dir_m2, d_time)
			
	def blips_to_mps(self, blips_m1, dir_m1, blips_m2, dir_m2, d_time):
		#print str(blips_m1) + " " + str(dir_m1) +" " + str(blips_m2) + " " + str(dir_m2) + " " + str(d_time)
# Step 1: Convert blips/s to m/s
		# Have number of blips turned in zzz zzz microseconds 
		#- lowest speed setting is ~150 blips per 500,000 microseconds (.5 seconds) 
		# want to calc ang wheel velocities for each wheel
		# How many blips in a rotation of the wheel?  
		# - Motor says 500 cpr, w/ gear ratio of 19.7:1. 
		#So, 500*19.7 = 9850 blips per rotation, bpr

		### xxx blips       1,000,000 mu.s     1 rotation 	2*math.pi radians
		### ------------- * -------------- * --------------- * -------------- ->
		### zzz,zzz mu.s      1 second		9850 blips	  1 rotation

		### ==>  ang_sp = (xxx blips/zzzzzz mu.s)*(101.5228)*2*math.pi radians per second

		### This would give speed of each wheel: 
		# There are 20,570 blips in a meter. Elapsed_time is in microseconds
		#	  xxx blips	       1 meter         1000000 mu.s
		#	----------------- * -------------- * ------------ = Xlin/elapsed_time m/s
		#	zzzzzz (mu.s)        20573 blips      1 second
		


		# angular speed of 1st motor wheel (right motor):
		omega_r = (blips_m1*101.5228*2*3.141529)/(d_time)
		# include directions
		if(dir_m1 == 0): # if wheel is turning backwards
			omega_r = -omega_r		

		# angular speed of 2nd motor wheel (left motor):
		omega_l = (blips_m2*101.5228*2*3.141529)/(d_time)	
		if(dir_m2 == 0): # if wheel is turning backwards
			omega_l = -omega_l		

			# This convention means: "front" is the side the wheels are on, "back" is the side the circuitry is on, 
			# "positive angular velocity" means Motor 2 is moving faster than Motor 1, "negative angular vel" means
			# motor 1 is moving faster (more positively) than motor 2
# Step 1.5: publish angular wheel velocities to 'wheel_vels' topic.
		wv = str(omega_l) + ' ' + str(omega_r)
		self.wPub.publish(wv) # publish just the wheel velocities (abs vals, dir not included) to topic
	

# Step 2: calc lin_speed and ang_speed for whole robot, 
		#given omegaR and omegaL of the wheels
		# Rr = 3 inches = Rl
		# D = 23.5 or something
		# sp.linear = (omega_r * Rr + omega_l * Rl) / 2
		# sp.angular = (omega_r * Rr - omega_l * Rl) / D

		# Calc linear spocity of whole system
		lin_speed = (omega_l + omega_r)*self.WHEEL_RADIUS/2

		# Calc angular spocity of whole system
		ang_speed = (omega_l - omega_r)*self.WHEEL_RADIUS/self.AXIS_DIST
		
		speeds = str(lin_speed) + " " + str(ang_speed)

		self.velPub.publish(speeds)	

		print "Speeds published | Linear: " + str(lin_speed) + ". Angular: " + str(ang_speed)

if __name__ == '__main__':
	vc = velocity_calculator()
	print "Creating new instance of velocity_calculator"
	vc.read_arduino()
	print "Beginning reading arduino serial input..."
